% 验证ecef2lla_cg与lla2ecef_cg函数
clc;
clear;

% 常数
load('earthconstants_WGS84.mat', 'RE', 'RP', 'e');

% 理论文档中ECEF系到matlab中ECEF系的DCM
C1 = [0 0 1; 1 0 0; 0 1 0];

% 理论文档中经纬度单位为弧度；matlab中经纬度单位为度
C2 = [180/pi 0 0; 0 180/pi 0; 0 0 1];

% 测试点1: 北纬0度 东经0度 高度400m
lla(1, :) = [0, 0, 400];

% 测试点2: 北纬0度 东经45度 高度0m
lla(2, :) = [0, pi/4, 0];

% 测试点3: 北纬0度 东经60度 高度100m
lla(3, :) = [0, pi/3, 100];

% 测试点4: 北纬30度 东经0度 高度0m
lla(4, :) = [pi/6, 0, 0];

% 分别使用matlab自带lla2ecef函数与lla2ecef_cg函数计算pos
pos_mt = lla2ecef(lla*C2, e, RE)*C1;
pos_cg = lla2ecef_cg(lla, e, RE);

% 分别使用matlab自带ecef2lla函数与ecef2lla_cg函数计算lla
lla_mt = ecef2lla(pos_mt*C1', e, RE)/C2;
lla_cg = ecef2lla_cg(pos_cg, e, RE);

% 输出比较结果
diff_pos = pos_cg - pos_mt
diff_lla = lla_mt - lla
diff_lla_cg = lla_cg - lla
